#pragma once
#include <ros/ros.h>
#include "common_lib/common.h"
#include "control/control_params.h"
#include "control/custom_class.h"
#include <tf/tf.h>
#include "amr_trajectory_msgs/amr_trajectory_msg.h"
#include "yaml-cpp/yaml.h"
#include "control/move_control/trajectory_process/trajectory_process_base.h"

namespace control
{
    class Trajectory : public TrajectoryProcessBase
    {
    public:
        Trajectory(){};
        ~Trajectory(){};
        void Init(ros::NodeHandle n) override;
        amr_trajectory_msgs::amr_trajectory_msg SearchOnlinePoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state) override;
        int SearchNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state) override;
        amr_trajectory_msgs::amr_trajectory_msg PickUpPath(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state,int min_index, float pick_up_length) override;
        amr_trajectory_msgs::point_info SearchAllMapNearestPoint(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state) override;
    private:
        std::string direction_;
		std::vector<float> min_point3d_;
		std::vector<float> min_diff_;
        common_lib::Common common_cal_;
        common_lib::TON no_online_ton_;
    };
}
